import * as THREE from "./three.js/build/three.module.js";
import Stats from '../jsm/libs/stats.module.js';
import { OrbitControls } from '../jsm/controls/OrbitControls.js';
import { GUI } from '../jsm/libs/dat.gui.module.js';

import { getContactObjects, getContactPoints } from "./A3Common.js";

// 配置参数
const settings = {

  // debug
  logPeriod: 1,          // 终端输出频率 单位s
  // graphics
  backgroundColor:0x1982cc,
  useRandomMaterial: true,

  // physics
  gravityConstant: -9.8,
  airDensityConstant: 0.4,
  margin:0.05,

}

const contactsMap = new Map();

// 物理引擎参数
let AmmoLib;
// let collisionConfiguration;
// let dispatcher;
// let softBodySolver;
let worldTransform;
let physicsWorld;
let dispatcher;

const WANTS_DEACTIVATION = 3;
const DISABLE_DEACTIVATION = 4;
const DISABLE_SIMULATION  = 5;

// 图形化界面常量
const SPI = Math.PI;
const DPI = SPI * 2;
const HPI = SPI * .5;

const A2R = 180.0 / Math.PI;
const R2A = Math.PI / 180.0;

const AxisX = new THREE.Vector3( 1, 0, 0 );
const AxisY = new THREE.Vector3( 0, 1, 0 );
const AxisZ = new THREE.Vector3( 0, 0, 1 );

// 参数列表
const params = { };

// 异步初始化Ammo
async function initAmmo() {

  if ( 'Ammo' in window === false ) {

    console.error( 'AmmoPhysics: Couldn\'t find Ammo.js' );
    return;

  }

  AmmoLib = await Ammo(); 

}

// 初始化物理世界
function initPhysics() {

  const gravityConstant = settings.gravityConstant;
  const airDensityConstant = settings.airDensityConstant;

  const collisionConfiguration = new AmmoLib.btSoftBodyRigidBodyCollisionConfiguration();
  // const collisionConfiguration = new AmmoLib.btDefaultCollisionConfiguration()
  dispatcher = new AmmoLib.btCollisionDispatcher( collisionConfiguration );
  const broadphase = new AmmoLib.btDbvtBroadphase();
  const solver = new AmmoLib.btSequentialImpulseConstraintSolver();
  const softBodySolver = new AmmoLib.btDefaultSoftBodySolver();
  
  physicsWorld = new AmmoLib.btSoftRigidDynamicsWorld( dispatcher, broadphase, solver, collisionConfiguration, softBodySolver );
  physicsWorld.setGravity( new AmmoLib.btVector3( 0, gravityConstant, 0 ) );
  physicsWorld.getWorldInfo().set_m_gravity( new AmmoLib.btVector3( 0, gravityConstant, 0 ) );
  physicsWorld.getWorldInfo().set_air_density ( airDensityConstant );

  // 设置prototype会导致物理模型频繁抖动？
  AmmoLib.btGImpactCollisionAlgorithm.prototype.registerAlgorithm(physicsWorld.getDispatcher());

  worldTransform = new AmmoLib.btTransform();

}

// 生成刚体物理模型
function createRigidBody( physicsShape, mass, position, quaternion, inertia ) {

  const transform = new AmmoLib.btTransform();
  transform.setIdentity();
  transform.setOrigin( new AmmoLib.btVector3( position.x, position.y, position.z ) );
  transform.setRotation( new AmmoLib.btQuaternion( quaternion.x, quaternion.y, quaternion.z, quaternion.w ) );
  const motionState = new AmmoLib.btDefaultMotionState( transform );
  
  const localInertia = new AmmoLib.btVector3( inertia.x, inertia.y, inertia.z );
  physicsShape.calculateLocalInertia( mass,  localInertia );

  const rbInfo = new AmmoLib.btRigidBodyConstructionInfo( mass, motionState, physicsShape, localInertia );
  const body = new AmmoLib.btRigidBody( rbInfo );

  if( mass > 0 ) {
    body.setActivationState( DISABLE_DEACTIVATION );
  }
  
  return body;

}

// 生成铰链约束
function createHingeConstraint( rbA, rbB, frameInA, frameInB, useReferenceFrameA=true ) {

  const hinge = new AmmoLib.btHingeConstraint( 
    rbA, rbB, 
    frameInA, frameInB,
    useReferenceFrameA 
  );
  
  return hinge;

}

// 设置视觉模型与物理模型的指向
function setUserPointer( threeObject, physicsBody ) {

  threeObject.userData.physicsBody = physicsBody;

  const btVecUserData = new AmmoLib.btVector3( 0, 0, 0 );
  btVecUserData.threeObject = threeObject;
  physicsBody.setUserPointer( btVecUserData );

}

// A3Class
class A3Class{

  // 仿真相关设置
  stopSimulation = false;
  pauseSimulation = false;

  actorsMap = new WeakMap();
  recyclers = [];
  recyclersMap = new WeakMap();

  // 外部渲染器
  extRenderers = [];
  extRenderersMap = new WeakMap();

  constructor( container ){

    if( isNull(container) ){
      container = document.createElement('div');
    }    

    this.container = container;
    this.AmmoLib = undefined;

    this.domElement = container;
    this.settings = settings;

    this.rigidBodiesMap = new WeakMap();
    this.constraintMap = new WeakMap();

  }
  ///////////////////////////////////////////////////////////////// 初始化相关函数 
  // 初始化A3
  async init(){

    // if ( 'Ammo' in window === false ) {

    //   console.error( 'AmmoPhysics: Couldn\'t find Ammo.js' );
    //   return false;
  
    // }
  
    // const AmmoLib = await Ammo(); 

    if( !AmmoLib ) return false;

    this.AmmoLib = AmmoLib;

    this.initGraphics();

    this.initStats();
    this.initGUI();

    initPhysics();
    this.world = physicsWorld;
    // this.initPhysics();

    this.initInput();

    this.clock = new THREE.Clock();
    this.lastLogTime = 0;
    this.rigidBodies = [];

    this.actors = [];

    return true;

  }

  // 初始化图形界面
  initGraphics() {

    const backgroundColor = this.settings.backgroundColor;
    const container = this.container;

    const scene = new THREE.Scene();
    scene.background = new THREE.Color( backgroundColor );

    // 坐标系辅助
    const axesHelper = new THREE.AxesHelper(1);
    scene.add(axesHelper);

    const renderer = new THREE.WebGLRenderer({ 
      alpha : true,
      preserveDrawingBuffer : true,
    });
    // renderer.setClearColor(0XFFFFFF, 0);
    renderer.setPixelRatio( window.devicePixelRatio );
    renderer.setSize( window.innerWidth, window.innerHeight );
    renderer.shadowMap.enabled = true;
    container.appendChild( renderer.domElement );


    const camera = new THREE.PerspectiveCamera(
      45, window.innerWidth / window.innerHeight, 
      0.1, 2000
    );

    camera.position.set( 10, 30, 10 );
    const controls = new OrbitControls( camera, renderer.domElement );
    controls.update();
    camera.userData.control = controls;

    const textureLoader = new THREE.TextureLoader();

    const ambientLight = new THREE.AmbientLight( 0xffffff, 0.6 );
    scene.add(ambientLight)

    const light = new THREE.DirectionalLight( 0xffffff, 0.9 );
    light.position.set( 0, 10, 20 );
    light.castShadow = true;
    const d = 10;
    light.shadow.camera.left = - d;
    light.shadow.camera.right = d;
    light.shadow.camera.top = d;
    light.shadow.camera.bottom = - d;

    light.shadow.camera.near = 2;
    light.shadow.camera.far = 50;

    light.shadow.mapSize.x = 1024;
    light.shadow.mapSize.y = 1024;
    // scene.add( light );

    //

    window.addEventListener( 'resize', this.onWindowResize.bind(this) );


    this.camera = camera;
    this.scene = scene;
    this.renderer = renderer;
    this.textureLoader = textureLoader;

  }

  // 初始化状态显示窗口
  initStats() {

    const container = this.container;

    const stats = new Stats();
    stats.domElement.style.position = 'absolute';
    stats.domElement.style.top = '0px';
    container.appendChild( stats.domElement );

    this.stats = stats;

  }

  // 初始化GUI
  initGUI() {

    const gui = new GUI( params );

    const sim = gui.addFolder( '仿真控制' );
    
    params["start"] = this.start.bind(this);
    params["stop"] = this.stop.bind(this);
    params["pause"] = this.pause.bind(this);
    params["resume"] = this.resume.bind(this);

    sim.add( params, "start" ).name("开始");
    sim.add( params, "stop" ).name("停止");
    sim.add( params, "pause" ).name("暂停");
    sim.add( params, "resume" ).name("继续");

    // sim.open();
    this.gui = gui;

  }

  // 初始化输入
  initInput() {

    window.addEventListener( 'keydown', this.onKeyDown.bind( this ) );

    window.addEventListener( 'keyup', this.onKeyUp.bind( this ) );

  }
  // // 初始化物理引擎
  // initPhysics() {
  //   // Physics configuration

  //   const AmmoLib = this.AmmoLib;
  //   const gravityConstant = this.settings.gravityConstant;
  //   const airDensityConstant = this.settings.airDensityConstant;

  //   const collisionConfiguration = new AmmoLib.btSoftBodyRigidBodyCollisionConfiguration();
  //   const dispatcher = new AmmoLib.btCollisionDispatcher( collisionConfiguration );
  //   const broadphase = new AmmoLib.btDbvtBroadphase();
  //   const solver = new AmmoLib.btSequentialImpulseConstraintSolver();
  //   const softBodySolver = new AmmoLib.btDefaultSoftBodySolver();
    
  //   const physicsWorld = new AmmoLib.btSoftRigidDynamicsWorld( dispatcher, broadphase, solver, collisionConfiguration, softBodySolver );
  //   physicsWorld.setGravity( new AmmoLib.btVector3( 0, gravityConstant, 0 ) );
  //   physicsWorld.getWorldInfo().set_m_gravity( new AmmoLib.btVector3( 0, gravityConstant, 0 ) );
  //   physicsWorld.getWorldInfo().set_air_density ( airDensityConstant );

  //   const worldTransform = new AmmoLib.btTransform();

  //   this.physicsWorld = physicsWorld;
  //   this.worldTransform = worldTransform;

  // }
  /////////////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////////// 常用函数
  // 获取转换
  getTransform ( pos, quat ) {

    const AmmoLib = this.AmmoLib;

    const transform = new AmmoLib.btTransform();
    transform.setIdentity();

    transform.setOrigin( new AmmoLib.btVector3( pos.x, pos.y, pos.z ) );
    transform.setRotation( new AmmoLib.btQuaternion( quat.x, quat.y, quat.z, quat.w ) );

    return transform;

  }
  /////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////// 约束相关函数
  // 铰链约束 hinge
  createHingeConstraint( rbA, rbB, pivotInA, pivotInB, axisInA, axisInB, useReferenceFrameA=true ) {
    
    // const AmmoLib = this.AmmoLib;
    // const physicsWorld = this.physicsWorld;

    if( isNull( pivotInA ) ){
      pivotInA = new THREE.Vector3( 0, 0, 0 );
    }

    if( isNull( pivotInB ) ){
      pivotInB = new THREE.Vector3( 0, 0, 0 );
    }
    
    if( isNull( axisInA ) ){
      axisInA = new THREE.Vector3( 0, 1, 0 );
    }

    if( isNull( axisInB ) ){
      axisInB = axisInA;
    }
    
    const hinge = new Ammo.btHingeConstraint( 
      rbA, rbB, 
      new AmmoLib.btVector3( pivotInA.x, pivotInA.y, pivotInA.z ),
      new AmmoLib.btVector3( pivotInB.x, pivotInB.y, pivotInB.z ),
      new AmmoLib.btVector3( axisInA.x, axisInA.y, axisInA.z ),
      new AmmoLib.btVector3( axisInB.x, axisInB.y, axisInB.z ),
      useReferenceFrameA 
    );

    physicsWorld.addConstraint( hinge, true );

    return hinge;

  }
  
  createHingeConstraint2( rbA, rbB, frameInA, frameInB, useReferenceFrameA=true ) {

    // const AmmoLib = this.AmmoLib;
    // const physicsWorld = this.physicsWorld;

    const hinge = new AmmoLib.btHingeConstraint( 
      rbA, rbB, 
      frameInA, frameInB,
      useReferenceFrameA 
    );
    
    physicsWorld.addConstraint( hinge, true );

    return hinge;
  }

  setHingeVelocity( hinge, velocity, impulse ) {

    hinge.enableAngularMotor( true, velocity, impulse );

  }

  ///////////////////////////////////////////////////////////////// 创建模型相关函数

  // 移除刚体
  removeRigidBody ( rigidBody ) {

    const index = this.rigidBodiesMap.get( rigidBody );
    
    this.scene.remove( rigidBody );
    physicsWorld.removeRigidBody( rigidBody.userData.physicsBody );

    // 释放资源
    disposeNode( rigidBody );

    this.rigidBodiesMap.delete( rigidBody );
    this.rigidBodies.splice( index, 1 );

  }

  // 创建刚体并添加到图形界面
  addRigidBody ( threeObject, physicsShape, mass, position, quaternion, inertia ) {

    // const AmmoLib = this.AmmoLib;
    // const physicsWorld = this.physicsWorld;
    const scene = this.scene;
    const rigidBodies = this.rigidBodies;
    const rigidBodiesMap = this.rigidBodiesMap;

    if( isNull(position) ) {
      position = threeObject.position;
    }
    else {
      threeObject.position.copy(position);
    }
    
    if( isNull(quaternion) ){
      quaternion = threeObject.quaternion;
    }
    else{
      threeObject.quaternion.copy(quaternion);
    }

    if( isNull( inertia ) ){
      inertia = new AmmoLib.btVector3( 0, 0, 0 );
    }
    // else {
    //   inertia = new AmmoLib.btVector3( inertia.x, inertia.y, inertia.z );
    // }
  
    const transform = new AmmoLib.btTransform();
    transform.setIdentity();
    transform.setOrigin( new AmmoLib.btVector3( position.x, position.y, position.z ) );
    transform.setRotation( new AmmoLib.btQuaternion( quaternion.x, quaternion.y, quaternion.z, quaternion.w ) );
    const motionState = new AmmoLib.btDefaultMotionState( transform );
  
    physicsShape.calculateLocalInertia( mass, inertia );
    // physicsShape.calculateLocalInertia( mass, new AmmoLib.btVector3( inertia.x, inertia.y, inertia.z ) );
  
    const rbInfo = new AmmoLib.btRigidBodyConstructionInfo( mass, motionState, physicsShape, inertia );
    const body = new AmmoLib.btRigidBody( rbInfo );
  
    threeObject.userData.physicsBody = body;
  
    threeObject.add( new THREE.AxesHelper() );    // 默认添加坐标轴工具

    scene.add( threeObject );
  
    if ( mass > 0 ) {
  
      rigidBodies.push( threeObject );
      rigidBodiesMap.set( threeObject, rigidBodies.length - 1 );
      // Disable deactivation
      body.setActivationState( DISABLE_DEACTIVATION );
  
    }
  
    // 设置用户指针指向网格模型
    const btVecUserData = new AmmoLib.btVector3( 0, 0, 0 );
    btVecUserData.threeObject = threeObject;
    body.setUserPointer( btVecUserData );

    physicsWorld.addRigidBody( body );
  
  }

  // 添加地面
  addGround ( width, height, depth, textureFile ) {

    const pos = new THREE.Vector3( 0, -height * 0.5, 0 );
    const quat = new THREE.Quaternion( 0, 0, 0, 1 );
    const ground = this.createParalellepiped( 
      width, height, depth, 0, pos, quat, new THREE.MeshPhongMaterial({ color: 0xcccccc })
    );

    ground.castShadow = true;
    ground.receiveShadow = true;
  
    this.textureLoader.load( textureFile, function ( texture ) {
  
      texture.wrapS = THREE.RepeatWrapping;
      texture.wrapT = THREE.RepeatWrapping;
      texture.repeat.set(  width * 2, length * 2 );
      ground.material.map = texture;
      ground.material.needsUpdate = true;
  
    } );

    // 修改相机位置便于观测
    this.camera.position.y = Math.max( width, depth );
  
  }

  // 由网格坐标生成凸包物理形状
  createConvexHullPhysicsShape( coords , scale=1 ) {

    const AmmoLib = this.AmmoLib;

    const shape = new AmmoLib.btConvexHullShape();
    var tempBtVec3_1 = new AmmoLib.btVector3( 0, 0, 0 );

    for ( let i = 0, il = coords.length; i < il; i += 3 ) {

      tempBtVec3_1.setValue( coords[ i ], coords[ i + 1 ], coords[ i + 2 ] );
      const lastOne = ( i >= ( il - 3 ) );
      shape.addPoint( tempBtVec3_1, lastOne );

    }

    shape.setLocalScaling(new AmmoLib.btVector3( scale, scale, scale ));

    return shape;

  }

  // 创建方体物理模型
  createParalellepiped( sx, sy, sz, mass, pos, quat, material ) {

    // const AmmoLib = this.AmmoLib;
    const margin = settings.margin;
    const useRandomMaterial = settings.useRandomMaterial;

    if( isNull(material) && useRandomMaterial ) {
      material = createMaterial();
    }

    const threeObject = new THREE.Mesh( new THREE.BoxGeometry( sx, sy, sz ), material );
    const shape = new AmmoLib.btBoxShape( new AmmoLib.btVector3( sx * 0.5, sy * 0.5, sz * 0.5 ) );
    shape.setMargin( margin );

    this.addRigidBody( threeObject, shape, mass, pos, quat );

    return threeObject;

  }

  // 创建球形模型
  createSphered( r, mass, pos, quat, material ) {

    const AmmoLib = this.AmmoLib;
    const margin = this.settings.margin;
    const useRandomMaterial = this.settings.useRandomMaterial;

    if( isNull(material) && useRandomMaterial ) {
      material = createMaterial();
    }

    const threeObject = new THREE.Mesh( new THREE.SphereGeometry( r ), material );
    const shape = new AmmoLib.btSphereShape( r );
    shape.setMargin( margin );

    this.addRigidBody( threeObject, shape, mass, pos, quat );

    return threeObject;

  }

  // 创建圆柱体模型
  createCylinder ( radius, height, mass, pos, quat, material ) {

    const AmmoLib = this.AmmoLib;
    const margin = this.settings.margin;
    const useRandomMaterial = this.settings.useRandomMaterial;

    if( isNull(material) && useRandomMaterial ) {
      material = createMaterial();
    }

    const threeObject = new THREE.Mesh( new THREE.CylinderGeometry( radius , radius, height ), material );
    const shape = new Ammo.btCylinderShape( new AmmoLib.btVector3( radius, height * 0.5, radius ) );
    shape.setMargin( margin );

    this.addRigidBody( threeObject, shape, mass, pos, quat );

    return threeObject;

  }

  // 创建圆锥体
  createCone ( radius, height, mass, pos, quat, material ) {

    const AmmoLib = this.AmmoLib;
    const margin = this.settings.margin;
    const useRandomMaterial = this.settings.useRandomMaterial;

    if( isNull(material) && useRandomMaterial ) {
      material = createMaterial();
    }

    const threeObject = new THREE.Mesh( new THREE.ConeGeometry( radius, height ), material );
		const shape = new AmmoLib.btConeShape( radius, height );
    shape.setMargin( margin );

    this.addRigidBody( threeObject, shape, mass, pos, quat );

    return threeObject;

  }

  // 创建组合形状
  addComoundShape( compound, pos, quat ) {

    this.addRigidBody( 
      compound.threeObject, 
      compound.shape, 
      compound.mass, 
      pos, quat,
      compound.inertia );

    return compound;

  }

  // 添加物体到图形界面
  addMesh( mesh ) {
    this.scene.add( mesh );
  }

  // 添加演员
  addActor( actor ) {

    switch( actor.type ) {
      case "recycler":
        this.addRecycler( actor );
        break;
      default:
        actor.addToWord( this );
        this.actors.push( actor );
        // this.actorsMap.set( actor, { actor:actor, index:this.actors.length-1 } );
        break;
    }
    
  }

  addRecycler( recycler ) {
    recycler.addToWord( this );
    
    this.recyclers.push( recycler );
    // this.recyclersMap.set( recycler, { actor:recycler, index:this.recyclers.length-1 } );

  }

  // 删除演员
  removeRecycler( recycler ) {

    // if( this.recyclersMap.has( recycler ) ) {
    //   const index = this.recyclersMap.get( recycler ).index;
    //   this.recyclers.splice( index, 1 );
    //   this.recyclersMap.delete( recycler );
    // }

    recycler.dispose();

  }

  // 删除演员
  removeActor( actor ) {

    // if( this.actorsMap.has( actor ) ) {
    //   const index = this.actorsMap.get( actor ).index;
    //   // this.actors.splice( index, 1 );
    //   this.actorsMap.delete( actor );
    // }

    actor.dispose();

  }

  /////////////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////////// 获取状态相关函数
  
  // 获取线速度
  getLinearVelocity( body ) {
    const lv = body.getLinearVelocity();

    return new THREE.Vector3( lv.x(), lv.y(), lv.z() );
  }

  // 获取角速度
  getAngularVelocity( body ) {
    const av = body.getAngularVelocity();

    return new THREE.Vector3( av.x(), av.y(), av.z() );
  }

  // 获取物体B相对于物体A的旋转信息
  getAngle( a, b, frameInA, frameInB , axisInA , axisInB ) {

    const ra = a.quaternion.clone();
    const rb = b.quaternion.clone();

    
    let quat ;

    if( !isNull(frameInA) ) {
      quat = frameInA.getRotation();
      const quatA = new THREE.Quaternion( quat.x(), quat.y(), quat.z(), quat.w() );
      ra.multiply( quatA );
    } 

    if( !isNull(frameInB) ) {
      quat = frameInB.getRotation();
      const quatB = new THREE.Quaternion( quat.x(), quat.y(), quat.z(), quat.w() );
      rb.multiply( quatB );
    }

    // console.log(ra, rb)

    return ra.angleTo(rb);

  }

  getHingeAngle( a, b, frameInA , frameInB, axis ) {

    const quatA = a.quaternion.clone();
    const quatB = b.quaternion.clone();
    const tmpQuat = new THREE.Quaternion();

    let quat ;
  

    if( !isNull(frameInA) ) {
      quat = frameInA.getRotation();
      tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
      quatA.multiply( tmpQuat );
    } 

    if( !isNull(frameInB) ) {
      quat = frameInB.getRotation();
      tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
      quatB.multiply( tmpQuat );
    }

    // const eulerA = new THREE.Euler().setFromQuaternion( quatA );
    // const eulerB = new THREE.Euler().setFromQuaternion( quatB );

    const X = quatA.invert().multiply( quatB );

    let order = "YXZ";

    if( axis &&  axis.x !== 0 ) {  order = "XYZ"; }

    if( axis &&  axis.z !== 0 ) { order = "ZXY"; };

    const euler = new THREE.Euler().setFromQuaternion( X , order);

    // console.log( euler.x * A2R, euler.y * A2R, euler.z * A2R );

    // return new THREE.Euler().setFromQuaternion( r );
    return euler;

  }

  // 更新物理世界位姿
  updateWorldTransform( body, pos, quat ){

    const AmmoLib = this.AmmoLib;
    const worldTransform = this.worldTransform;

    worldTransform.setOrigin( new AmmoLib.btVector3( pos.x, pos.y, pos.z ) );
    worldTransform.setRotation( new AmmoLib.btQuaternion( quat.x, quat.y, quat.z, quat.w ) );
    body.setWorldTransform( worldTransform );

  }
  /////////////////////////////////////////////////////////////////
  // 窗口大小改变事件
  onWindowResize() {

    const camera = this.camera;
    const renderer = this.renderer;

    camera.aspect = window.innerWidth / window.innerHeight;
    camera.updateProjectionMatrix();

    renderer.setSize( window.innerWidth, window.innerHeight );

  }
  
  // 键盘控制时间
  onKeyDown( event ) {
    // console.log( event )
  }

  onKeyUp( event ) {

  }
  ///////////////////////////////////////////////////////////////// 更新相关函数
  // 更新图形界面
  updateGraphics() {

    const renderer = this.renderer;
    const camera = this.camera;
    const scene = this.scene;

    renderer.setViewport( 0, 0,  window.innerWidth, window.innerHeight );
    renderer.render( scene, camera ); 
  
    if(camera.userData.control){
      camera.userData.control.update();
    }

  }

  // 更新物理世界
  updatePhysics( delta ){

    // const physicsWorld = this.physicsWorld;
    // const time = performance.now();

    physicsWorld.stepSimulation( delta );

    this.updateRigidBodyies();
    this.updateRecyclers();
    this.updateActors();

    // 更新碰撞表
    this.updateContacts();
  }

  // 更新刚体
  updateRigidBodyies() {

    const rigidBodies = this.rigidBodies;
    // const worldTransform = this.worldTransform;

    for ( var i = 0, il = rigidBodies.length; i < il; i++ ) {
      
      const objThree = rigidBodies[ i ];
      const objPhys = objThree.userData.physicsBody;
      const ms = objPhys.getMotionState();
      
      if ( ms ) {

        ms.getWorldTransform( worldTransform );
        const p = worldTransform.getOrigin();
        const q = worldTransform.getRotation();
        objThree.position.set( p.x(), p.y(), p.z() );
        objThree.quaternion.set( q.x(), q.y(), q.z(), q.w() );

      }

    }

  }

  // 更新回收器：优先级高于演员
  updateRecyclers() {
    this.recyclers.forEach( recycler => {
      recycler.update();
    })
  }

  // 更新演员
  updateActors() {

    this.actors.forEach( actor=>{
      actor.update();
    } )
    // for( var i = 0, il = this.actors.length; i < il; i++ ) {
    //   if( this.actorsMap.has( this.actors[i] ) )
    //     this.actors[i].update();
    // }
  }

  // 更新日志输出
  updateLog( delta ){
    
    this.lastLogTime += delta;

    if( this.lastLogTime >= this.settings.logPeriod ) {

      this.updateUsers();
      this.lastLogTime = 0;

    }

  }

  // 更新用户设置
  updateUsers( delta ){

  }

  // 更新碰撞表
  updateContacts() {

    contactsMap.clear();

    for ( let i = 0, il = dispatcher.getNumManifolds(); i < il; i ++ ) {

      const contactManifold = dispatcher.getManifoldByIndexInternal( i );
      const contacts = getContactObjects( contactManifold );
      
      
      if(isNull(contacts) || isNull(contacts.actor0) ||isNull(contacts.actor1) ) {
        continue;
      }
      
      const actor0 = contacts.actor0;
      const actor1 = contacts.actor1;

      const contactPoints = getContactPoints( contactManifold );

      const contact0 = { actor: actor0, contactPoints:[] };
      const contact1 = { actor: actor1, contactPoints:contactPoints }

      if( !contactsMap.has(actor0) ) {
        contactsMap.set( actor0, [
          contact1
        ] );
      }
      else{
        contactsMap.set( actor0, [ ...contactsMap.get( actor0 ), contact1 ] );
      }

      if( !contactsMap.has(actor1) ) {
        contactsMap.set( actor1, [contact0] );
      }
      else{
        contactsMap.set( actor1, [ ...contactsMap.get( actor1 ), contact0 ] );
      }
        
    }
    
  }
  
  update(){

    const delta = this.clock.getDelta();

    this.updateGraphics();

    if ( !this.pauseSimulation && !this.stopSimulation ){
    
      this.updatePhysics( delta );
    
    }
    
    this.stats.update();

    this.updateLog( delta );

    this.updateExtRenderers();

  }
  /////////////////////////////////////////////////////////////////

  // 动画
  start() {

    this.stopSimulation = false;
    this.pauseSimulation = false;
    this.animate();

  }

  stop() {

    this.stopSimulation = true;

  }

  pause() {
    
    this.pauseSimulation = true;

  }

  resume(){

    this.pauseSimulation = false;

  }

  animate() {

    requestAnimationFrame( this.animate.bind(this) );
    this.update();

  }
  //////////////////////////// 图像处理
  getImage( camera, width, height ) {

    camera = isNull( camera ) ? this.camera : camera;
    width = isNull( width ) ? window.innerWidth : width;
    height = isNull( height ) ? window.innerHeight : height;


    const renderer = new THREE.WebGLRenderer({ 
      preserveDrawingBuffer : true,
    });

    renderer.setPixelRatio( window.devicePixelRatio );
    renderer.setSize( width, height );
    renderer.shadowMap.enabled = true;

    camera.aspect = width / height;
    camera.updateProjectionMatrix();

    renderer.render( this.scene, camera );

    const img = new Image();
    img.src = renderer.domElement.toDataURL();
    
    return img;
  }

  getImageData( camera, width, height ) {

    camera = isNull( camera ) ? this.camera : camera;
    width = isNull( width ) ? window.innerWidth : width;
    height = isNull( height ) ? window.innerHeight : height;


    const renderer = new THREE.WebGLRenderer({ 
      preserveDrawingBuffer : true,
    });

    renderer.setPixelRatio( window.devicePixelRatio );
    renderer.setSize( width, height );
    renderer.shadowMap.enabled = true;

    camera.aspect = width / height;
    camera.updateProjectionMatrix();

    renderer.render( this.scene, camera );
    
    return renderer.domElement;
  }

  addRenderer( renderer, camera ) {

    this.extRenderers.push({ renderer: renderer, camera: camera });
    this.extRenderersMap.set( renderer, this.extRenderers.length - 1 );
    
  }

  removeRenderer( renderer ) {
    if( this.extRenderersMap.has( renderer ) ) {
      const index = this.extRenderersMap.get( renderer );
      this.extRenderers.splice( index, 1 );
      this.extRenderersMap.delete( renderer );
    }
  }

  updateExtRenderers(){

    for( var i = 0, il = this.extRenderers.length; i < il; i++ ) {
      
      const renderer = this.extRenderers[i].renderer;
      const camera = this.extRenderers[i].camera;
      
      camera.updateProjectionMatrix();
      renderer.render( this.scene, camera );

      // console.log(".")
    }

  }
  ////////////////////////////
}

//////////////////////////////////////////////// 物理图形相关函数

// 创建凸包物理形状
function createConvexHullPhysicsShape( coords , scale=1 ) {

  const shape = new AmmoLib.btConvexHullShape();
  var tempBtVec3_1 = new AmmoLib.btVector3( 0, 0, 0 );

  for ( let i = 0, il = coords.length; i < il; i += 3 ) {

    tempBtVec3_1.setValue( coords[ i ], coords[ i + 1 ], coords[ i + 2 ] );
    const lastOne = ( i >= ( il - 3 ) );
    shape.addPoint( tempBtVec3_1, lastOne );

  }

  shape.setLocalScaling(new AmmoLib.btVector3( scale, scale, scale ));

  return shape;

}

// 获取网格模型的物理形状
function getShape( geometry ) {

  // const AmmoLib = this.AmmoLib;
  const margin = settings.margin;

  const parameters = geometry.parameters;

  let shape = undefined;

  if (geometry.isBufferGeometry){

    const type = geometry.type;

    switch(type){

      case "BoxGeometry":

        var width = parameters.width !== undefined ? parameters.width / 2 : 0.5;
        var height = parameters.height !== undefined ? parameters.height / 2 : 0.5;
        var depth = parameters.depth !== undefined ? parameters.depth / 2 : 0.5;

        shape = new AmmoLib.btBoxShape( new AmmoLib.btVector3( width, height, depth ) );
        shape.setMargin( margin );
        break;

      case "ConeGeometry":
  
        var radius = parameters.radius !== undefined ? parameters.radius : 1;
        var height = parameters.height !== undefined ? parameters.height : 1;

        shape = new AmmoLib.btConeShape( radius, height );
        shape.setMargin( margin );

        break;

      case "CylinderGeometry":

        var height = parameters.height !== undefined ? parameters.height / 2 : 0.5;
        const radiusTop = parameters.radiusTop !== undefined ? parameters.radiusTop : 1;
        const radiusBottom = parameters.radiusBottom !== undefined ? parameters.radiusBottom : 1;

        shape = new AmmoLib.btCylinderShape( new AmmoLib.btVector3( radiusTop, height, radiusBottom ) );
        shape.setMargin( margin );

        break;
      
      case "SphereGeometry":

        var radius = parameters.radius !== undefined ? parameters.radius : 1;

        shape = new AmmoLib.btSphereShape( radius );
        shape.setMargin( margin );

        break;

      default:

        shape = createConvexHullPhysicsShape( geometry.attributes.position.array );          
        shape.setMargin(margin);

        break;
    }

  }
  else {

    console.error("不支持的几何形状:", geometry);

  }

  return shape;

}

////////////////////////////////////////////////
// 生成随机材质
function createMaterial( color , transparent=false, wireframe=false) {

  return new THREE.MeshPhongMaterial( { 
    color: color?color:createRandomColor(), 
    transparent: transparent, 
    opacity: 0.5,
    wireframe:wireframe
  } );

}

function createRandomColor() {

  return Math.floor( Math.random() * ( 1 << 24 ) );

}

/////////////////////////////////////////////// 常用函数

function isNull( obj ){
  return ( obj === null || obj === undefined ) ;
}

function disposeNode( node ) {

  for( var i = 0, il = node.children.length; i < il; i++ ) {
    disposeNode( node.children[i] );
  }

  if( node.geometry ) {
    node.geometry.dispose();
  }

  if( node.material ) {
    node.material.dispose();
  }

}

// 添加坐标轴
function addAxesHelper ( mesh, size=1 ) {
  
  const axes = new THREE.AxesHelper( size );
  mesh.add(axes);

}

// 关闭模型中的灯光
function disableMeshLight( node ){

  if(node.children){
    for(let i=0, il=node.children.length; i < il; i++){
      const child = node.children[i];
      if(child.children){
        disableMeshLight(child);
      }
    }
  }

  if(node.type === 'DirectionalLight'){
    node.visible = false;
  }

}
///////////////////////////////////////////////////
// 组合形状
class CompoundShape {

  constructor() {

    const compound = new AmmoLib.btCompoundShape();
    const transform = new AmmoLib.btTransform();
    transform.setIdentity();

    this.threeObject = new THREE.Mesh();
    this.shape = compound;
    this.mass = 0;
    this.inertia = new AmmoLib.btVector3(0, 0, 0);
    this.transform = transform;

  }

  addChild( mesh, shape,  mass, pos, quat ) {

    this.transform.setOrigin( new AmmoLib.btVector3( pos.x, pos.y, pos.z ) );
    this.transform.setRotation( new AmmoLib.btQuaternion( quat.x, quat.y, quat.z, quat.w ) );

    this.shape.addChildShape( this.transform, shape );

    mesh.position.copy( pos );
    mesh.quaternion.copy( quat );
    this.threeObject.add( mesh );

    this.mass += mass;
    this.shape.calculateLocalInertia( this.mass, this.inertia );
  }

}

// 演员类
class Actor {
  name = "actor";

  constructor() {

    // 默认演员

    this.threeObject = undefined;    // 视觉模型
    this.physicsBody = undefined;    // 碰撞模型

    this.needsUpdate = false;
    this.position = new THREE.Vector3( 0, 0, 0 );
    this.quaternion = new THREE.Quaternion( 0, 0, 0, 1 );
    this.inertia = new THREE.Vector3(0, 0, 0);
    this.mass = 0;

  }

  setFromMesh( mesh, mass, position, quaternion, inertia ) {

    const shape = getShape( mesh.geometry );

    if( isNull(position) ) {
      position = mesh.position.clone();
    }
    else {
      mesh.position.copy( position );
    }

    if( isNull(quaternion) ) {
      quaternion = mesh.quaternion.clone();
    }
    else {
      mesh.quaternion.copy( quaternion );
    }

    if( isNull( inertia ) ) {
      inertia = this.inertia;
    }

    if( !isNull( shape ) ) {

      const body = createRigidBody( shape, mass, position, quaternion, inertia );
    
      setUserPointer( mesh, body );
      addAxesHelper( mesh );  // for test

      this.threeObject = mesh;
      this.physicsBody = body;

    }

    this.position.copy( position );
    this.quaternion.copy( quaternion );
    this.mass = mass;

  }

  addToWord( scene, world ) {

    scene.add( this.threeObject );

    // if( this.mass > 0 ) {
    //   this.physicsBody.setActivationState ( DISABLE_DEACTIVATION );
    // }

    world.addRigidBody( this.physicsBody );

  }

  setAngularVelocity( av ) {
    this.physicsBody.setAngularVelocity( new AmmoLib.btVector3( av.x, av.y, av.z ));
  }

  setLinearVelocity( lv ) {
    this.physicsBody.setLinearVelocity( new AmmoLib.btVector3( lv.x, lv.y, lv.z ));
  }

  setPosition( position ) {

    this.position.copy( position );
    this.needsUpdate = true;

  }

  setQuaternion( quaternion ) {

    this.quaternion.copy( quaternion );
    this.needsUpdate = true;

  }

  update() {

    if( !this.physicsBody ) return;

    const ms = this.physicsBody.getMotionState();
    
    if ( ms ) {

      if( this.needsUpdate ) {
        
        worldTransform.setOrigin( new AmmoLib.btVector3( this.position.x, this.position.y, this.position.z ) );
        worldTransform.setRotation( new AmmoLib.btQuaternion( this.quaternion.x, this.quaternion.y, this.quaternion.z, this.quaternion.w ) );
        this.physicsBody.setWorldTransform( worldTransform );
        this.threeObject.position.copy( this.position );
        this.threeObject.quaternion.copy( this.quaternion );

        this.needsUpdate = false;
      }
      else {
        
        ms.getWorldTransform( worldTransform );
        const p = worldTransform.getOrigin();
        const q = worldTransform.getRotation();
        
        this.threeObject.position.set( p.x(), p.y(), p.z() );
        this.threeObject.quaternion.set( q.x(), q.y(), q.z(), q.w() );
        
        this.position.copy( this.threeObject.position );
        this.quaternion.copy( this.threeObject.quaternion );
      }

    }

  }

}

// 关节类演员
class A3HingeConstraint extends Actor {
  name = 'hinge';

  useReferenceFrameA = true;

  showHinge = false;

  // 速度相关
  velocity = 0.0;
  impulse = SPI;

  maxImpulse = DPI;
  maxSpeed = SPI;
  maxAcceleration = SPI;

  // 运动相关
  targetCoil = 0.0;
  targetAngle = 0.0;
  targetSpeed = 0.0;
  acceleration = 0.0;

  accuracy = SPI / 180;

  startAngle = 0.0;
  targetAngle = 0.0;
  startTime = 0.0;
  passTime = 0.0;

  needsUpdate = false;

  // 角度限制
  limit = [ -DPI, DPI ];

  constructor( parent, child ) {

    super();

    this.threeObject = new THREE.Group();

    const hingeGeometry = new THREE.CylinderGeometry( .1, .1, 1.1 );

    const visual = new THREE.Mesh( 
      hingeGeometry, 
      new THREE.MeshPhongMaterial( { color: 0x1982cc, transparent: true, opacity: 0.8 } )
    );

    visual.quaternion.setFromAxisAngle( AxisX, HPI );

    this.threeObject.add( visual );

    // addAxesHelper( this.threeObject, 1.2 );

    this.parent = isNull( parent ) ? new Actor() : parent;
    this.child = isNull( child ) ? new Actor() : child ;
    this.constraint = undefined;

    this.frameInA = new AmmoLib.btTransform();
    this.frameInA.setIdentity();
    this.frameInA.setOrigin(0, 2, 0)

    this.frameInB = new AmmoLib.btTransform();
    this.frameInB.setIdentity();
    
  }

  setFromFrame( frameInA, frameInB ) {

    if( isNull(frameInA) ) {
      frameInA = this.frameInA;
    }
    else{
      this.frameInA = frameInA;
    }

    if( isNull( frameInB ) ) {
      frameInB = this.frameInB;
    }
    else{
      this.frameInB = frameInB;
    }

    const constraint = createHingeConstraint( 
      this.parent.physicsBody,
      this.child.physicsBody,
      frameInA, frameInB,
      this.useReferenceFrameA
    );

    // constraint.enableAngularMotor( true, 0, 1 );

    this.constraint = constraint;

  }

  // 计算从旋转轴Z到目标旋转轴的旋转四元数
  calculateQuaternion( toAxis, fromAxis ) {
    
    // todo: 使用数学方式计算四元数的值
    const tmpQuat = new THREE.Quaternion( 0, 0, 0, 1 );

    if( toAxis.x === 1 ) {
      tmpQuat.setFromAxisAngle( AxisY, HPI );
    }

    if( toAxis.x === -1 ) {
      tmpQuat.setFromAxisAngle( AxisY, -HPI );
    }

    if( toAxis.y === 1 ) {
      tmpQuat.setFromAxisAngle( AxisX, -HPI );
    }

    if( toAxis.y === -1 ) {
      tmpQuat.setFromAxisAngle( AxisX, HPI );
    }

    if( toAxis.z === -1 ) {
      tmpQuat.setFromAxisAngle( AxisX, SPI );
    }

    return tmpQuat;

  }

  // 根据目标角度和当前角度计算速度
  calculateVelocity() {

    var currAngle = this.getHingeAngle();

    if( Math.abs( currAngle ) < this.accuracy ) {
      currAngle = 0.0;
    }

    currAngle = currAngle >= 0 ? currAngle : DPI + currAngle; // to [0, 2 * PI]

    const diffAngle = this.targetAngle - currAngle;

    if( Math.abs( diffAngle ) <= this.accuracy ) {

      this.velocity = 0.0
      this.impulse = this.maxImpulse;

      const endTime = performance.now();
      console.log( `Use time: ${endTime - this.startTime}s` );

      this.needsUpdate = false;
      
      return;

    }

    // console.log("Update velocity:",currAngle, this.targetAngle, diffAngle );

    if( diffAngle > 0 ) {
      this.velocity = this.targetSpeed;
    }
    else {
      this.velocity = -this.targetSpeed;
    }

  }

  // 使用锚点和旋转轴设置约束
  setFromPivot( pivotInA, pivotInB, axisInA, axisInB ) {
    const frameInA = this.frameInA;
    const frameInB = this.frameInB;

    if( isNull( pivotInA ) ) {
      pivotInA = new THREE.Vector3( 0, 0, 0 );
    }

    if( isNull( pivotInB ) ) {
      pivotInB = new THREE.Vector3( 0, 0, 0 );
    }

    frameInA.setOrigin( new AmmoLib.btVector3( pivotInA.x, pivotInA.y, pivotInA.z ) );
    frameInB.setOrigin( new AmmoLib.btVector3( pivotInB.x, pivotInB.y, pivotInB.z ) );
    
  
    if( !isNull(axisInA) ){
      
      const quatA = this.calculateQuaternion( axisInA );
      frameInA.setRotation( new AmmoLib.btQuaternion( quatA.x, quatA.y, quatA.z, quatA.w ) );

    }

    if( !isNull(axisInB) ){
      
      const quatB = this.calculateQuaternion( axisInB );
      frameInB.setRotation( new AmmoLib.btQuaternion( quatB.x, quatB.y, quatB.z, quatB.w ) );

    }

    // const constraint = createHingeConstraint2( 
    //   this.parent.physicsBody,
    //   this.child.physicsBody,
    //   pivotInA, pivotInB, axisInA, axisInB,
    //   this.useReferenceFrameA
    // );

    // this.constraint = constraint;

    this.setFromFrame();

  }

  // 获取关节角度
  getHingeAngle() {

    const threeObjA = this.parent.threeObject;
    const threeObjB = this.child.threeObject;

    const quatA = threeObjA.quaternion.clone();
    const quatB = threeObjB.quaternion.clone();
    const tmpQuat = new THREE.Quaternion();

    let quat;

    quat = this.frameInA.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    quatA.multiply( tmpQuat );

    quat = this.frameInB.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    quatB.multiply( tmpQuat );

    const X = quatA.invert().multiply( quatB );

    const euler = new THREE.Euler().setFromQuaternion( X , 'ZXY');

    return euler.z;

  }

  // 设置角度
  setHingeAngle( angle ) {
    
    const threeObjA = this.parent.threeObject;
    // const threeObjB = this.child.threeObject;

    const quatA = threeObjA.quaternion.clone();
    // const quatB = threeObjB.quaternion.clone();
    const tmpQuat = new THREE.Quaternion();

    let quat;

    const X = new THREE.Quaternion().setFromAxisAngle( AxisZ, angle );

    quat = this.frameInA.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    quatA.multiply( tmpQuat );

    quat = this.frameInB.getRotation();
    tmpQuat.set( quat.x(), quat.y(), quat.z(), quat.w() );
    // quatB.multiply( tmpQuat );

    quatA.multiply(X).multiply( tmpQuat );

    this.child.setQuaternion( quatA );

  }

  // 添加到仿真世界
  addToWord( scene, world ) {

    scene.add( this.threeObject );

    // if( this.mass > 0 ) {
    //   this.physicsBody.setActivationState ( DISABLE_DEACTIVATION );
    // }

    world.addConstraint( this.constraint, true );

  }

  // 移动至角度
  moveTo( angle, speed, acceleration) {

    this.targetCoil = Math.floor( angle / DPI );
    angle = angle % DPI;

    this.targetAngle = angle > 0 ? angle : DPI + angle;

    // 确保速度和加速度为正
    speed = Math.abs(speed);
    acceleration = Math.abs(acceleration);
    
    if ( isNull(speed) || speed > this.maxSpeed ) {
      this.targetSpeed = this.maxSpeed;
    }
    else {
      this.targetSpeed = speed;
    }

    if ( isNull( acceleration) || acceleration > this.maxAcceleration ){
      this.acceleration = this.maxAcceleration;
    }
    else {
      this.acceleration = acceleration;
    }
    
    this.startTime = performance.now();

    this.needsUpdate = true;

  }

  // 设置速度
  setHingeVelocity( velocity, impulse ) {

    this.velocity = isNull( velocity )? 0 : velocity;
    this.impulse  = isNull( impulse ) ? 0 : impulse;

    this.needsUpdate = false;
  }

  // 停止
  stopHinge( impulse ) {

    this.velocity = 0.0;

    if( !isNull( impulse ) ) {
      this.impulse = impulse;
    }

    this.needsUpdate = false;

  }

  update() {

    if( this.showHinge ) {
      // update hinge visual
      this.position.copy( this.parent.position );
      this.quaternion.copy( this.parent.quaternion );

      const p = this.frameInA.getOrigin();
      const q = this.frameInA.getRotation();

      const tmpPos = new THREE.Vector3( p.x(), p.y(), p.z() );
      const tmpQuat = new THREE.Quaternion( q.x(), q.y(), q.z(), q.w() );

      tmpPos.applyQuaternion( this.quaternion );
      this.quaternion.multiply( tmpQuat );

      this.position.add( tmpPos );

      this.threeObject.position.copy( this.position );
      this.threeObject.quaternion.copy( this.quaternion );
    }

    this.threeObject.visible = this.showHinge;

    if( this.needsUpdate ) {

      this.calculateVelocity();

    }

    this.constraint.enableAngularMotor( true, this.velocity, this.impulse );

  }

}

const BasicShapeTypes = {
  "Box":"Box"
}

// 使用[长, 宽, 高, 重量, 材质]生成BoxMesh
function createBoxMesh( parameters ) {

  const sx = parameters[0] ? parameters[0] : 1;
  const sy = parameters[1] ? parameters[1] : 1;
  const sz = parameters[2] ? parameters[2] : 1;
  const mass = parameters[3] ? parameters[3] : 0;
  const material = parameters[4] ? parameters[4] : createMaterial();

  const margin = settings.margin;

  const position = new THREE.Vector3( 0, sy * 0.5, 0 );
  const quaternion = new THREE.Quaternion( 0, 0, 0, 1 );

  const threeObject = new THREE.Mesh( new THREE.BoxGeometry( sx, sy, sz ), material );
  const shape = new AmmoLib.btBoxShape( new AmmoLib.btVector3( sx * 0.5, sy * 0.5, sz * 0.5 ) );
  shape.setMargin( margin );

  const body = createRigidBody( shape, mass, position, quaternion, new THREE.Vector3( 0, 0, 0) );

  setUserPointer( threeObject, body );

  return threeObject;

}

// 形状类演员
class A3BasicShape extends Actor {

  constructor( type="Box", ...parameters ) {
    super();

    switch( type ){
      default:
        this.threeObject = createBoxMesh( parameters );
        this.physicsBody = this.threeObject.userData.physicsBody;
        break;
    }

  }

}

/////////////////////////////////////////////////// 工具相关函数

// URDF助手
// 由URDF创建碰撞模型
class URDFHelper  extends Actor {
  name = "URDFActor";

  constructor(robot, scale) {
    super();

    robot.scale.set(scale, scale, scale);

    this.threeObject = robot;
    this.scale = new THREE.Vector3( scale, scale, scale );

    this.init();

  }

  addToWord (scene, world) {

    const robot = this.threeObject;

    scene.add( this.threeObject );

    for (var key in robot.colliders) {

      const collider = robot.colliders[key];
      const objPhys = collider.userData.physicsBody;

      objPhys.setActivationState( DISABLE_SIMULATION );
      world.addRigidBody(objPhys);

    }

    this.scene = scene;

  }

  update() {

    const robot = this.threeObject;

    this.scene.updateMatrixWorld(true);

    for (var key in robot.colliders) {

      const collider = robot.colliders[key];
      const objPhys = collider.userData.physicsBody;
      
      collider.matrixWorld.decompose( this.position, this.quaternion, this.scale );

      worldTransform.setOrigin( new AmmoLib.btVector3( this.position.x, this.position.y, this.position.z ) );
      worldTransform.setRotation( new AmmoLib.btQuaternion( this.quaternion.x, this.quaternion.y, this.quaternion.z, this.quaternion.w ) );
      objPhys.setWorldTransform( worldTransform );

    }

  }

  init(){

    const robot = this.threeObject;
    const scale = this.scale;

    for (var key in robot.links) {

      const link = robot.links[key];

      if (link.children.length <= 2)
        continue;

      const collider = link.children[1];

      // console.log( key, collider, link );
      var mass = 0;
      var inertia = new THREE.Vector3(0, 0, 0);

      if (link.inertial) {
        mass = link.inertial.mass;
        inertia.set(link.inertial.inertia[0], link.inertial.inertia[1], link.inertial.inertia[2]);
      }

      const mesh = collider.children[0].children[0];
      const shape = getShape(mesh.geometry);
      
      shape.setLocalScaling( new AmmoLib.btVector3( scale.x, scale.y, scale.z ) );

      const body = createRigidBody(shape, mass, collider.position, collider.quaternion, inertia);
      setUserPointer(collider, body);

      // collider.visible = true;

    }

  }
}

///////////////////////////////////////////////////

A3Class.prototype.CompoundShape = CompoundShape;
A3Class.prototype.Actor = Actor;
A3Class.prototype.A3HingeConstraint = A3HingeConstraint;
A3Class.prototype.A3BasicShape = A3BasicShape;
A3Class.prototype.BasicShapeTypes = BasicShapeTypes;

A3Class.prototype.initAmmo = initAmmo;
A3Class.prototype.initPhysics = initPhysics;
A3Class.prototype.createMaterial = createMaterial;
A3Class.prototype.getShape = getShape;
A3Class.prototype.addAxesHelper = addAxesHelper;

A3Class.prototype.AxisX = AxisX;
A3Class.prototype.AxisY = AxisY;
A3Class.prototype.AxisZ = AxisZ;

A3Class.prototype.disableMeshLight = disableMeshLight;
A3Class.prototype.URDFHelper = URDFHelper;


export default A3Class;
export { 
  
  initAmmo,
  AmmoLib,
  Actor, 

  contactsMap,
  
};


